//
// RS Game Framework
// Copyright © 2009 Jedd Haberstro
// jhaberstro@gmail.com
// 
// $Id:
//

#ifndef RS_LOCKFREEQUEUE_HPP
#define RS_LOCKFREEQUEUE_HPP

#include "rs/Atomic.hpp"
#include "rs/MetaFacilities.hpp"
#include "rs/Portability.hpp"
#include <cstddef>

namespace rs
{
    namespace concurrency
    {
        // Fober et al queue
        // Not 64-bit safe - assumes 32 bit wide pointers
        template< typename T >
        class LockFreeQueue
        {
        public:
            
			typedef T ValueType;
	
        public:
            
            struct Cell
            {
                Cell() {
                }

                Cell(T const& value)
                : value(value) {
                }

                Cell* volatile next;
                T value;
            };
            
        public:
                        
            LockFreeQueue();
            
            void Push(Cell* newCell);
            
            Cell* Pop();
            
            Int32 GetSize() const;
            
        private:
            LockFreeQueue(LockFreeQueue const&);
            LockFreeQueue& operator=(LockFreeQueue const&);
            
            Cell* EndQueue();
            
        private:
            
            Cell* volatile head_;
            UInt32 volatile ocount_;
            Cell* volatile tail_;
            UInt32 volatile icount_;
            Int32 size_;
            Cell dummy_;
        };
        
        
        
        template< typename T >
        inline LockFreeQueue< T >::LockFreeQueue()
        : size_(0) {
            RS_STATIC_ASSERT(sizeof(head_) == 4, head_is_not_sizeof_4);
            RS_STATIC_ASSERT(sizeof(tail_) == 4, tail_is_not_sizeof_4);
            RS_STATIC_ASSERT(sizeof(ocount_) == 4, ocount_is_not_sizeof_4);
            RS_STATIC_ASSERT(sizeof(icount_) == 4, icount_is_not_sizeof_4);
            RS_STATIC_ASSERT(offsetof(LockFreeQueue< T >, ocount_) == (offsetof(LockFreeQueue< T >, head_) + 4), ocount_offset_is_incorrect);
            RS_STATIC_ASSERT(offsetof(LockFreeQueue< T >, icount_) == (offsetof(LockFreeQueue< T >, tail_) + 4), icount_offset_is_incorrect);
            dummy_.next = EndQueue();
            head_ = tail_ = &dummy_;
        }
        
        template< typename T >
        void LockFreeQueue< T >::Push(Cell* newCell) {
            newCell->next = EndQueue();
            Cell* tail;
            UInt32 icount;
            while (true) {
                icount = icount_;
                tail = tail_;
                if (atomic::CompareSwap(reinterpret_cast< UInt32 volatile* >(&tail->next), reinterpret_cast< UInt32 >(EndQueue()), reinterpret_cast< UInt32 >(newCell))) {
                    break;
                }
                
                UInt64 oldValue = reinterpret_cast< UInt64 >(tail) | (static_cast< UInt64 >(icount) << 32);
                UInt64 newValue = reinterpret_cast< UInt64 >(tail->next) | (static_cast< UInt64 >(icount + 1) << 32);
                atomic::CompareSwap(reinterpret_cast< UInt64 volatile* >(&tail_), oldValue, newValue);
            }
            
            UInt64 oldValue = reinterpret_cast< UInt64 >(tail) | (static_cast< UInt64 >(icount) << 32);
            UInt64 newValue = reinterpret_cast< UInt64 >(newCell) | (static_cast< UInt64 >(icount + 1) << 32);
            atomic::CompareSwap(reinterpret_cast< UInt64 volatile* >(&tail_), oldValue, newValue);
            atomic::AddFetch(&size_, 1);
        }
        
        template< typename T >
        typename LockFreeQueue< T >::Cell* LockFreeQueue< T >::Pop() {
            UInt32 ocount;
            UInt32 icount;
            Cell* head;
            Cell* next;
            
            while (true) {
                ocount = ocount_;
                icount = icount_;
                head = head_;
                next = head->next;
                if (ocount == ocount_) {
                    if (head == tail_) {
                        if (next == EndQueue()) {
                            return 0;
                        }
                        
                        UInt64 oldValue = reinterpret_cast< UInt64 >(head) | (static_cast< UInt64 >(icount) << 32);
                        UInt64 newValue = reinterpret_cast< UInt64 >(next) | (static_cast< UInt64 >(icount + 1) << 32);
                        atomic::CompareSwap(reinterpret_cast< UInt64 volatile* >(&tail_), oldValue, newValue);
                    }
                    else if (next != EndQueue()) {
                        UInt64 oldValue = reinterpret_cast< UInt64 >(head) | (static_cast< UInt64 >(ocount) << 32);
                        UInt64 newValue = reinterpret_cast< UInt64 >(next) | (static_cast< UInt64 >(ocount + 1) << 32);
                        if (atomic::CompareSwap(reinterpret_cast< UInt64 volatile* >(&head_), oldValue, newValue)) {
                            break;
                        }
                    }
                }
            }
            
            atomic::AddFetch(&size_, -1);
            if (head == &dummy_) {
                Push(head);
                head = Pop();
            }
            
            return head;
        }
        
        template< typename T >
        inline Int32 LockFreeQueue< T >::GetSize() const {
            return size_;
        }
        
        template< typename T >
        inline typename LockFreeQueue< T >::Cell* LockFreeQueue< T >::EndQueue() {
            return reinterpret_cast< Cell* >(this);
        }
    }
}

#endif // RS_LOCKFREEQUEUE_HPP
